import time
import pigpio
from .motor_driver import *


class Ultrasonic:
    VALUE_TIMEOUT = 20000

    def __init__(self, trigger, echo):
        """
        The class is instantiated with the Pi to use and the
        gpios connected to the trigger and echo pins.
        """
        self.pi = pigpio.pi()
        self._trig = trigger
        self._echo = echo

        self._ping = False
        self._high = None
        self._time = None

        self._triggered = False

        self._trig_mode = self.pi.get_mode(self._trig)
        self._echo_mode = self.pi.get_mode(self._echo)

        self.pi.set_mode(self._trig, pigpio.OUTPUT)
        self.pi.set_mode(self._echo, pigpio.INPUT)

        self._cb = self.pi.callback(self._trig, pigpio.EITHER_EDGE, self._cbf)
        self._cb = self.pi.callback(self._echo, pigpio.EITHER_EDGE, self._cbf)

        self._inited = True
        self.dist_mov_ave = 0
        self.avoid_distance = 20  # 避撞距离
        self.distance = 0

    def _cbf(self, gpio, level, tick):
        if gpio == self._trig:
            if level == 0:  # trigger sent
                self._triggered = True
                self._high = None
        else:
            if self._triggered:
                if level == 1:
                    self._high = tick
                else:
                    if self._high is not None:
                        self._time = tick - self._high
                        self._high = None
                        self._ping = True

    def read(self):
        """
        round trip cms = round trip time / 1000000.0 * 34030
        """
        if self._inited:
            self._ping = False
            self.pi.gpio_trigger(self._trig)
            start = time.time()
            while not self._ping:
                if (time.time()-start) > 5.0:
                    return Ultrasonic.VALUE_TIMEOUT
                time.sleep(0.00001)
            return self._time
        else:
            return None

    def cancel(self):
        """
        Cancels the ranger and returns the gpios to their
        original mode.
        """
        if self._inited:
            self._inited = False
            self._cb.cancel()
            self.pi.set_mode(self._trig, self._trig_mode)
            self.pi.set_mode(self._echo, self._echo_mode)

    def get_distance(self):
        round_trip_time = self.read()
        if round_trip_time != Ultrasonic.VALUE_TIMEOUT and round_trip_time != 0:
            return round_trip_time * 0.034030/2
        else:
            return Ultrasonic.VALUE_TIME

    def __exit__(self, exc_type, exc_value, traceback):
        self.cancel()
        self.pi.stop()

    def distance_moving_average(self):
        for _ in range(100):
            time.sleep(0.001)
            dist_current = self.get_distance()
            # if the sensor missed the echo, the output dis_mov_ave will equal the last dis_mov_ave
            if dist_current != Ultrasonic.VALUE_TIMEOUT:
                # using the moving average of distance measured by sensor to reduce the error
                self.dist_mov_ave = 0.8*dist_current + 0.2*self.dist_mov_ave
                break

    def worker(self, motor):
        """ul thread worker function"""
        while True:
            time.sleep(0.01)
            self.distance_moving_average()
            self.distance = self.dist_mov_ave
            # print ("Measured Distance = %.1f cm" % dist)
            if (motor.MOTOR_STATE == Motor.FORWARD or motor.MOTOR_STATE == Motor.LEFT or motor.MOTOR_STATE == Motor.RIGHT):
                if (self.distance!=0.0):
                    if (self.distance < self.avoid_distance):
                        motor.action(Motor.STOP)
              
